#!/bin/sh /etc/rc.common

#------------------------------------------------------------------------------
#
#  Init script for EtherCAT
#
#  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
#  Copyright (C) 2020  Anton Kikin, Tano Systems LLC
#
#------------------------------------------------------------------------------

START=60
STOP=20

EXTRA_COMMANDS="status"

extra_command "mstatus" "Checks for EtherCAT master status"

MODPROBE=/usr/sbin/insmod
RMMOD=/usr/sbin/rmmod
ETHERCAT=/usr/bin/ethercat
MASTER_ARGS=

#------------------------------------------------------------------------------

parse_mac_address() {
	if [ -z "${1}" ]; then
		MAC=""
	elif echo ${1} | grep -qE '^([0-9a-fA-F]{2}:){5}[0-9a-fA-F]{2}$'; then
		MAC=${1}
	else
		echo Invalid MAC address \"${1}\" in ${ETHERCAT_CONFIG}
		exit 1
	fi
}

#------------------------------------------------------------------------------

DEVICE_MODULES=""
MODPROBE_FLAGS=""
DEVICES=""
BACKUPS=""
MASTER_COUNT=0

config_read_global() {
	config_get DEVICE_MODULES $1 'module' ''
	config_get MODPROBE_FLAGS $1 'modprobe_flags' ''
}

config_read_master() {
	local device
	local backup

	config_get device $1 'device' ''
	config_get backup $1 'backup' ''

	if [ -z "${device}" ]; then return; fi

	if [ ${MASTER_COUNT} -gt 0 ]; then
		DEVICES=${DEVICES},
		BACKUPS=${BACKUPS},
	fi

	parse_mac_address ${device}
	DEVICES=${DEVICES}${MAC}

	parse_mac_address ${backup}
	BACKUPS=${BACKUPS}${MAC}

	MASTER_COUNT=$(expr ${MASTER_COUNT} + 1)
}

config_read() {
	DEVICE_MODULES=""
	MODPROBE_FLAGS=""
	DEVICES=""
	BACKUPS=""
	MASTER_COUNT=0

	config_load "ethercat"
	config_foreach config_read_global ethercat

	# construct DEVICES and BACKUPS from configuration variables
	config_foreach config_read_master master
}

start() {
	config_read

	if [ "${MASTER_COUNT}" = "0" ]; then
		echo "Error: No masters configured"
		exit 1
	fi

	# load master module
	if ! ${MODPROBE} ${MODPROBE_FLAGS} ec_master ${MASTER_ARGS} \
		main_devices=${DEVICES} backup_devices=${BACKUPS}; then
		exit 1
	fi

	LOADED_MODULES=ec_master

	# check for modules to replace
	for MODULE in ${DEVICE_MODULES}; do
		ECMODULE=ec_${MODULE}
		if [ ${MODULE} != "generic" ]; then
			if lsmod | grep "^${MODULE} " > /dev/null; then
				if ! ${RMMOD} ${MODULE}; then
					${RMMOD} ${LOADED_MODULES}
					exit 1
				fi
			fi
		fi
		if ! ${MODPROBE} ${MODPROBE_FLAGS} ${ECMODULE}; then
			if [ ${MODULE} != "generic" ]; then
				${MODPROBE} ${MODPROBE_FLAGS} ${MODULE} # try to restore
			fi
			${RMMOD} ${LOADED_MODULES}
			exit 1
		fi

		LOADED_MODULES="${ECMODULE} ${LOADED_MODULES}"
	done
}

stop() {
	config_read

	# unload EtherCAT device modules
	for MODULE in ${DEVICE_MODULES} master; do
		ECMODULE=ec_${MODULE}
		if ! lsmod | grep -q "^${ECMODULE} "; then
			continue # ec_* module not loaded
		fi
		if ! ${RMMOD} ${ECMODULE}; then
			exit 1
		fi;
	done

	sleep 1

	# reload previous modules
	for MODULE in ${DEVICE_MODULES}; do
		if [ ${MODULE} != "generic" ]; then
			if ! ${MODPROBE} ${MODPROBE_FLAGS} ${MODULE}; then
				echo "Warning: Failed to restore ${MODULE}."
			fi
		fi
	done
}

restart() {
	stop
	sleep 1
	start
}

reload() {
	restart
}

mstatus() {
	echo "Checking for EtherCAT masters..."

	config_read

	RESULT=0

	for i in `seq 0 $(expr ${MASTER_COUNT} - 1)`; do
		echo -n "Master${i} "

		# Check if the master is in idle or operation phase
		${ETHERCAT} master --master ${i} 2>/dev/null | \
			grep -qE 'Phase:[[:space:]]*Idle|Phase:[[:space:]]*Operation'
		EXITCODE=$?

		if [ ${EXITCODE} -eq 0 ]; then
			echo "running"
		else
			echo "dead"
			RESULT=1
		fi
	done

	exit ${RESULT}
}

#------------------------------------------------------------------------------
